(load "package://pr2eus_moveit/euslisp/collision-object-publisher.l")
(load "models/foldable-desk-object.l")
(load "models/petbottle-object.l")

(defun init-collision-demo (&optional (robot *sr*) (link-name "WAIST_LINK0"))
  (setq *co* (instance collision-object-publisher :init))

  (setq *table* (instance foldable-desk-object :init))
  (setq *bottle* (instance petbottle-object :init))
  (send *table* :translate #f(1000.0 0.0 0.0))
  (send *bottle* :translate #f(900.0 140.0 705.0))

  (setq *robot* robot)
  (setq *link-name* link-name)
  (objects (list *robot* *table* *bottle*))
  )

(defun pub-objects ()
  (pub-obj *table*)
  (pub-obj *bottle*)
  )

(defun clear-objects ()
  (send *co* :clear-all)
  )

(defun pub-obj (obj &optional (robot *robot*) (link-name *link-name*)) ;; need link
  (let ((cds (send (send robot
                         (intern (format nil "~A_LK" (string-upcase link-name))
                                 *keyword-package*) :copy-worldcoords)
                   :transformation (send obj :worldcoords))))
    (send *co* :add-object obj :frame-id link-name :relative-pose cds)
    obj))

(warn "
(init-collision-demo *your-robot* \"ROOT_LINKNAME\")
(pub-objects)
;; (clear-objects)
")
